function node_path = a_star_search(q_init,q_goal,g,map,n,node_info)
%% A_STAR_SEARCH 
% Implementation of the A* algortihm for searching the correct path

init = map(q_init(1),q_init(2));
goal = map(q_goal(1),q_goal(2));

if (init == 0 || goal == 0)
    node_path = [];
    return
end

grey   = zeros(1,n);
father = zeros(1,n);
step   = 1;

grey(init) = 1;
queue = [ init; 0 ];

node = queue(1,1);
queue(:,1) = [];

while (node ~= goal)
    
    children = find(g(node,:));
    
    for i = 1:length(children)
        if (grey(children(i)) == 0)
            grey(children(i))   = 1;
            father(children(i)) = node;
            queue = enqueue(children(i),(step + heuristics(q_goal,children(i),node_info)),queue);
        end
    end
    
    step = step + 1;
    
    if isempty(queue)
        node_path = [];
        return
    else
        node = queue(1,1);
        queue(:,1) = [];
    end
end

node = goal;
node_path = node;

while (node ~= init)
    node = father(node);
    node_path = [ node, node_path ];
    
end

end

function queue = enqueue(node,node_cost,queue)

for i = 1:length(queue(1,:))  
    if node_cost < queue(2,i)
        queue = [ queue(:,1:(i - 1)), [ node; node_cost ], queue(:,i:length(queue(1,:))) ];
        break
    end
end

if (length(queue(1,:)) == length(queue(1,:)))
    queue(:,end + 1) = [ node; node_cost ];
end

end

function h = heuristics(q_goal,node,node_info)

q(1) = round((node_info(node,1) + node_info(node,2) - 1)/2);
q(2) = round((node_info(node,3) + node_info(node,4) - 1)/2);

h = norm(q_goal - q);

end